
#include <stdlib.h>
#include <stdarg.h>
#include <string.h>
#include "bluetooth.h"

extern UART_HandleTypeDef huart3;

static T_Command _parse_usart3_command (char *str);
	
T_USART_Buffer usart3;
static char uart3_tx_buffer[128];
uint8_t BUFFER_LENGTH1=127;

void UART3_Printf(const char* format,...)
{
    int i=-1;

    //uint8_t result;


    va_list arg;
    va_start(arg,format);
    i=vsnprintf((char *)&uart3_tx_buffer,BUFFER_LENGTH1,format,arg);
    va_end(arg);

		HAL_UART_Transmit_DMA(&huart3, (uint8_t *)&uart3_tx_buffer, i);

}

int usart3_reset_buffer (void)
{
    for (int i=0; i<USART_BUF_SIZE; i++)
    {
        usart3.rx_buffer[i] = 0x00;
    }
    usart3.rx_index = 0;
    
    if (usart3.rx_buffer[0] == 0x00)
    {
        return USART_BUF_SIZE;
    }
    else
    {
        return -1;
    }
}

int usart3_enqueue_to_buffer (uint8_t data)
{

		usart3.rx_buffer[usart3.rx_index++] = data;
		if (usart3.rx_index > USART_BUF_SIZE)
		{
				usart3.rx_index = 0;
		}

    return 1;           

}

static T_Command _parse_usart3_command (char *str)
{
	T_Command cmd;
	cmd.action = COMMAND_NOTHING;
	cmd.speed_amount = 0;
	char *token;
	

	token=strtok(str,",");
	if(strcmp(token,"bo")==0)
	{
		cmd.action=CMD_BLUETOOTH_OPEN;
	}
	else if(strcmp(token,"bs")==0)
	{
		cmd.action=CMD_BLUETOOTH_CLOSE;
	}
	else if(strcmp(token,"bf")==0)
	{
		cmd.action=CMD_BLUETOOTH_FORWARD;
	}
	else if(strcmp(token,"bb")==0)
	{
		cmd.action=CMD_BLUETOOTH_BACKWARD;
	}
	else if(strcmp(token,"bccw")==0)
	{
		cmd.action=CMD_BLUETOOTH_CCW;
	}
	else if(strcmp(token,"bcw")==0)
	{
		cmd.action=CMD_BLUETOOTH_CW;
	}
	else if(strcmp(token,"bsl")==0)
	{
		cmd.action=CMD_BLUETOOTH_SHIFTLEFT;
	}
	else if(strcmp(token,"bsr")==0)
	{
		cmd.action=CMD_BLUETOOTH_SHIFTRIGHT;
	}
	cmd.speed_amount=atoi(strtok(NULL,","));
	return cmd;
}
T_Command lscm_parse_received_command_usart3 (void)
{
    T_Command cmd;
    
    cmd = _parse_usart3_command ((char *)usart3.rx_buffer);
    
    return cmd;
}


Joystick_Command lscm_parse_received_command_usart3_joystick (void)
{
    Joystick_Command cmd;
    
    cmd = _parse_usart3_joystick_command ((char *)usart3.rx_buffer);
    
    return cmd;
}

static Joystick_Command _parse_usart3_joystick_command (char *str)
{
	Joystick_Command cmd;
	cmd.mode=NOTHING;
	cmd.action=COMMAND_NOTHING;
	
	switch(*str)
	{
		case 'U':
			cmd.Radius_Left=(int)*(++str);
		  cmd.Angle_Left=(int)*(++str);
			cmd.Radius_Right=(int)*(++str);
			cmd.Angle_Right=(int)*(++str);
		if(cmd.Radius_Right>0||cmd.Angle_Right>0)
			cmd.mode=SPIN_MODE;
		else
			cmd.mode=SHIFT_MODE;
		
		cmd.action=CMD_BLUETOOTH_FORWARD;
		
			break;
		case 'A':
			cmd.action=CMD_BLUETOOTH_OPEN;
			break;
		case 'B':
			cmd.action=CMD_BLUETOOTH_CLOSE;
			break;
		case 'C':
			break;
		case 'D':
			break;
		default:
			cmd.action=CMD_ERROR;
			break;
		
	
	}

	return cmd;	


}

